# coding : UTF-8
"""
作者：BingBO   时间：2022年10月23日
自动调整代码格式 ：Alt+Ctrl+L
"""
import math

import cv2 as cv
import numpy as np
from skimage import morphology  # 形态学操作，如开闭运算、骨架提取等


def skeleton(img):
    """
    skeleton(img[, params]) -> retval
    @brief 对原始红色条纹图像 提取骨架.
    .   @param img：输入3通道原始条纹图像.
    .   @retval skeleton：(单通道)骨架图像.
    .   @retval threshold：(单通道)条纹 阈值图像.
    """
    height, width, channels = img.shape
    print("输入h: %s, w: %s, c:%s" % (height, width, channels))
    _, _, red = cv.split(img)

    _, threshold = cv.threshold(red, 160, 255, cv.THRESH_BINARY)  # 转为二值图
    threshold = cv.medianBlur(threshold, 3)
    cv.imshow("medianBlur", threshold)
    cv.imwrite(r"F:\MVS_Data\threshold.png", threshold)

    # 骨架提取
    NormalizedImg = threshold / 255  # 像素值归一化
    skeleton = morphology.skeletonize(NormalizedImg)  # 骨架提取
    skeleton = skeleton.astype(np.uint8) * 255

    cv.imwrite(r"F:\MVS_Data\skeleton.png", skeleton)
    return skeleton


def CrossPointlocation(skeleton):
    """
    CrossPointDetect(skeleton[, params]) -> retval
    @brief 对骨架图像 计算 交叉中心点坐标.
    .   @param skeleton：输入1通道骨架图像.
    .   @retval CrossPoint：交叉中心点 图像坐标（row, col）or（y，x）.
    """
    h, w = skeleton.shape
    img = skeleton / 255  # 归一化图像
    Tmax = 0
    for row in range(5, h - 5):
        for col in range(5, w - 5):
            if img[row, col] == 1:
                T = 0
                for k in range(2, 5):
                    T += img[row + k, col + k] + img[row - k, col + k] + img[row + k - 1, col + k] + img[
                        row - k + 1, col + k] + img[row + k, col - k] + img[row - k, col - k] + img[
                             row + k - 1, col - k] + img[row - k + 1, col - k]
                for m in range(-1, 2):
                    for n in range(-1, 2):
                        T += img[row + m, col + n]
                if T >= Tmax:
                    Tmax = T
                    CrossPoint = (row, col)
    print("CrossPoint坐标：", CrossPoint)
    skeleton3 = np.zeros([h, w, 3], np.uint8)
    skeleton3[:, :, 0] = skeleton
    skeleton3[:, :, 1] = skeleton
    skeleton3[:, :, 2] = skeleton
    skeleton3[CrossPoint] = [0, 0, 255]
    cv.imwrite(r"F:\MVS_Data\CrossPoint.png", skeleton3)
    return CrossPoint


# 垂直灰度重心法
# 传入的是RGB 截取图像
# upperRowIndex ： 窗口上边界在图像中的行索引
def wds_Roi(wds, upperRowIndex):
    h, w, c = wds.shape
    Red = wds[:, :, 2]  # B0 G1 R2
    _, thrRed = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
    thrRed = cv.medianBlur(thrRed, 3)
    gravityCoorList = []  # 中心点的坐标列表
    for col in range(w):  # 0 ，1 …… ，w-1
        n = 0  # 存储该列有无光点
        sum_RowIdx = 0  # 该列的中心点行索引
        for row in range(h):
            if thrRed[row, col] != 0:
                n += 1
                sum_RowIdx += row + 1
        if n:
            gravityRow = int(sum_RowIdx / n) - 1
            realRow = upperRowIndex + gravityRow  # 在原始图像中的行坐标
            gravityCoorList.append((col, realRow))  # 添加坐标（列，行）
    return gravityCoorList


# 交叉点 初略定位
def CrossPointlocation_step1(img):
    imgcopy = img.copy()  # imgcopy 用来展示窗口效果
    high, wth, chs = img.shape
    h, w = 40, 20
    rowIndexUp, rowIndexDn = 0, high - h  # 都是窗口的上边界的行索引
    findUp, findDn = 0, 0
    wdsnum = 1
    # 1. 搜索窗口迭代
    while findUp == 0:
        wdsUp = img[rowIndexUp:rowIndexUp + h, 0: w]
        cv.rectangle(imgcopy, (0, rowIndexUp), (w - 1, rowIndexUp + h - 1), (0, 255, 0), 1)  # 在图像内部 最外圈
        Red = wdsUp[:, :, 2]  # B0 G1 R2
        _, thrRed = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        thrRed = cv.medianBlur(thrRed, 3)
        '''
        # 查看图像
        cv.imshow("thrRed_Up", thrRed)
        cv.waitKey(0)
        cv.destroyAllWindows()
        '''
        # 判断当前窗口图像  是否存在光条
        sumX = np.sum(thrRed, axis=1)  # 1 行相加, 变成一列
        sum = np.sum(sumX, axis=0)  # 0 列相加
        if sum != 0:
            findUp = 1
            sumXIndex = np.where(sumX > 0)  # 返回（索引+ 数据类型 ）
            rowUpIdx = min(sumXIndex[0])
            findRow = rowIndexUp + rowUpIdx
            print("交叉点定位：当前窗口检测到光条，最上行索引：", findRow)
            # 定位窗口
            cv.rectangle(imgcopy, (0, findRow - 19), (19, findRow + 40), (255, 0, 255), 1)  # 在图像内部最外圈（宽20 高60）
            fitWdsUp = img[findRow - 19:findRow + 41, 0: w]
            wdsUp_row = findRow - 19
        else:
            print("交叉点定位：当前窗口%d未检测到光条" % wdsnum)
        rowIndexUp += h
        wdsnum += 1
    wdsnum = 1
    while findDn == 0:
        wdsDn = img[rowIndexDn:rowIndexDn + h, 0: w]
        cv.rectangle(imgcopy, (0, rowIndexDn), (w - 1, rowIndexDn + h - 1), (0, 255, 0), 1)
        Red2 = wdsDn[:, :, 2]  # B0 G1 R2
        _, thrRed2 = cv.threshold(Red2, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        thrRed2 = cv.medianBlur(thrRed2, 3)
        '''
        # 查看图像
        cv.imshow("thrRed_Dn", thrRed2)
        cv.waitKey(0)
        cv.destroyAllWindows()
        '''
        # 判断当前窗口图像  是否存在光条
        sumX = np.sum(thrRed2, axis=1)  # 1 行相加, 变成一列
        sum = np.sum(sumX, axis=0)  # 0 列相加
        if sum != 0:
            findDn = 1
            sumXIndex = np.where(sumX > 0)  # 返回（索引+ 数据类型 ）
            rowUpIdx = min(sumXIndex[0])
            findRow = rowIndexDn + rowUpIdx
            print("交叉点定位：当前窗口检测到光条，最上行索引：", findRow)
            # 定位窗口
            cv.rectangle(imgcopy, (0, findRow - 19), (19, findRow + 40), (255, 0, 255), 1)  # 在图像内部最外圈（宽20 高60）
            fitWdsDn = img[findRow - 19:findRow + 41, 0: w]
            wdsDn_row = findRow - 19
        else:
            print("交叉点定位：当前窗口%d未检测到光条" % wdsnum)
        rowIndexDn -= h
        wdsnum += 1

    cv.imshow("fitWdsUp", fitWdsUp)
    cv.imshow("fitWdsDn", fitWdsDn)
    # 对窗口处理，拟合出直线斜率
    gravityCoorUp = wds_Roi(fitWdsUp, wdsUp_row)  # list类型
    gravityCoorDn = wds_Roi(fitWdsDn, wdsDn_row)  # list类型

    print("gravityCoorUp:", gravityCoorUp)
    print("gravityCoorDn:", gravityCoorDn)
    coorarray_up = np.array(gravityCoorUp)
    coorarray_dn = np.array(gravityCoorDn)

    # 对中心点1 拟合
    pointSet1_col = coorarray_up[:, 0]  # 列
    pointSet1_row = coorarray_up[:, 1]  # 行
    poly = np.polyfit(pointSet1_col, pointSet1_row, 1)  # 用最小二乘法（含COG_V误差）拟合得到的直线参数
    fit1_k, fit1_b = poly[0], poly[1]  # A, B, C = fit_k, -1, fit_b
    print(fit1_k, fit1_b)  # b = 122

    p1 = gravityCoorUp[0]  # 列，行
    p2 = (400, int(fit1_k * 400 + fit1_b))
    print(p2)
    cv.line(imgcopy, p1, p2, (255, 255, 0), 1)  # 青蓝

    # 对中心点2 拟合
    pointSet2_col = coorarray_dn[:, 0]  # 列
    pointSet2_row = coorarray_dn[:, 1]  # 行
    poly = np.polyfit(pointSet2_col, pointSet2_row, 1)  # 用最小二乘法（含COG_V误差）拟合得到的直线参数
    fit2_k, fit2_b = poly[0], poly[1]  # A, B, C = fit_k, -1, fit_b
    print(fit2_k, fit2_b)  # b = 122

    p3 = gravityCoorDn[0]  # 列，行
    p4 = (400, int(fit2_k * 400 + fit2_b))
    print(p4)
    cv.line(imgcopy, p3, p4, (255, 255, 0), 1)  # 青蓝

    # 3. 计算交点坐标，并设置交叉点ROI
    cx = int((fit2_b - fit1_b) / (fit1_k - fit2_k))
    cy = int(fit1_k * cx + fit1_b)
    cv.circle(imgcopy, (cx, cy), 2, (255, 0, 255), -1)
    cv.rectangle(imgcopy, (cx - 80, cy - 40), (cx + 80, cy + 40), (0, 255, 0), 1)  # 列，行

    cv.imshow("imgshow", imgcopy)
    cv.imwrite(r"F:\MVS_Data\edgeSearch.png", imgcopy)
    return cx, cy  # 列 行


# 对搜索小窗口内进行 交叉点定位
# img：截取的 小窗口图像
def CrossPointlocation_step2(img, lw):
    src = img.copy()
    cx, cy = CrossPointlocation_step1(img)
    roih = 80
    roiw = 160
    roi_row = img[(cy - int(roih / 2)): (cy + int(roih / 2)), (cx - int(roiw / 2)):(cx + int(roiw / 2))]  # 行， 列
    cv.imshow("roi", roi_row)
    # 列，行  实际大小+1
    cv.rectangle(img, (cx - int(roiw / 2), cy - int(roih / 2)), (cx + int(roiw / 2), cy + int(roih / 2)), (0, 255, 0),
                 1)

    h, w, _ = roi_row.shape
    print((h, w))
    Red = roi_row[:, :, 2]  # B0 G1 R2
    _, threshold = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
    # threshold2 = cv.adaptiveThreshold(Red, 255, cv.ADAPTIVE_THRESH_MEAN_C, cv.THRESH_BINARY, 13, 2)
    roi = cv.medianBlur(threshold, 3)
    cv.imshow("roi_threshold", roi)
    # print(img[130, 190])

    # 直接对red通道进行计算
    sumMax = 0
    halfLw = int(lw / 2)
    for row in range(0, h):
        for col in range(0, w):
            sum = 0
            # lw: 激光条纹的垂直方向 宽度
            for rowAdd in range(0, halfLw * 3 - 1):
                for colAdd in range(0, halfLw):
                    # l1右
                    if 0 <= (row + rowAdd) < h and 0 <= (col + rowAdd + colAdd) < w:
                        sum += roi[row + rowAdd, col + rowAdd + colAdd]
                    if 0 <= (row + rowAdd) < h and 0 <= (col + rowAdd - colAdd) < w:
                        sum += roi[row + rowAdd, col + rowAdd - colAdd]
                    # l1左
                    if 0 <= (row - rowAdd) < h and 0 <= (col - rowAdd + colAdd) < w:
                        sum += roi[row - rowAdd, col - rowAdd + colAdd]
                    if 0 <= (row - rowAdd) < h and 0 <= (col - rowAdd - colAdd) < w:
                        sum += roi[row - rowAdd, col - rowAdd - colAdd]
                    # l2右
                    if 0 <= (row - rowAdd) < h and 0 <= (col + rowAdd + colAdd) < w:
                        sum += roi[row - rowAdd, col + rowAdd + colAdd]
                    if 0 <= (row - rowAdd) < h and 0 <= (col + rowAdd - colAdd) < w:
                        sum += roi[row - rowAdd, col + rowAdd - colAdd]
                    # l2左
                    if 0 <= (row + rowAdd) < h and 0 <= (col - rowAdd + colAdd) < w:
                        sum += roi[row + rowAdd, col - rowAdd + colAdd]
                    if 0 <= (row + rowAdd) < h and 0 <= (col - rowAdd - colAdd) < w:
                        sum += roi[row + rowAdd, col - rowAdd - colAdd]
            if sum >= sumMax:
                sumMax = sum
                cpRoi = (col, row)
    print("CrossPoint在roi中的精确坐标：", cpRoi)
    cv.circle(roi_row, cpRoi, 2, (255, 0, 255), -1)

    cpCol = cx - int(roiw / 2) + cpRoi[0]
    cpRow = cy - int(roih / 2) + cpRoi[1]
    CrossPoint = (cpCol, cpRow)
    print("CrossPoint在全图中的精确坐标：", CrossPoint)
    # 提取出Laser1、Laser2、以及四个区域
    regionROI, laserL1, laserL2 = regionSegment(src, CrossPoint)

    cv.imshow("CrossPoint", roi_row)
    cv.imwrite(r"F:\MVS_Data\CrossPoint.png", roi_row)
    return CrossPoint, laserL1, laserL2


def regionSegment(src, CrossPoint):
    """
    regionSegment(src, CrossPoint[, params]) -> retval
    @brief 基于交叉点坐标对原始输入图像进行分割.
    .   @param src：输入3通道原始条纹图像.
    .   @param CrossPoint：交叉中心点 图像坐标（row, col）or（y，x）.
    .   @retval regionROI：分割四个区域（左上、左下、右上、右下）.
    .   @retval laserL1：条纹1组合图像（左上 + 右下）.
    .   @retval laserL2：条纹2组合图像（左下 + 右上）.
    """
    crossRow, crossCol = CrossPoint[1], CrossPoint[0]
    LeftUpRoi = src[0:crossRow, 0:crossCol]
    LeftDownRoi = src[crossRow:, 0:crossCol]
    RightUpRoi = src[0:crossRow, crossCol:]
    RightDownRoi = src[crossRow:, crossCol:]
    # cv.imwrite(r"F:\MVS_Data\LeftUpRoi.png", LeftUpRoi)
    # cv.imwrite(r"F:\MVS_Data\LeftDownRoi.png", LeftDownRoi)
    # cv.imwrite(r"F:\MVS_Data\RightUpRoi.png", RightUpRoi)
    # cv.imwrite(r"F:\MVS_Data\RightDownRoi.png", RightDownRoi)
    regionROI = [LeftUpRoi, LeftDownRoi, RightUpRoi, RightDownRoi]  # 4个子区域列表

    laserL1 = np.zeros_like(src)
    laserL2 = np.zeros_like(src)
    laserL1[0:crossRow, 0:crossCol] = LeftUpRoi
    laserL1[crossRow:, crossCol:] = RightDownRoi
    laserL2[crossRow:, 0:crossCol] = LeftDownRoi
    laserL2[0:crossRow, crossCol:] = RightUpRoi
    # cv.imshow("laserL1", laserL1)
    cv.imwrite(r"F:\MVS_Data\laser1.png", laserL1)
    # cv.imshow("laserL2", laserL2)
    cv.imwrite(r"F:\MVS_Data\laser2.png", laserL2)
    return regionROI, laserL1, laserL2


# 边缘点定位
def edgePointlocation(imgUp, imgDn):
    print("------------边缘检测---------------")
    cv.imshow("input_Up", imgUp)
    cv.imshow("input_Dn", imgDn)
    srcUp = imgUp.copy()  # src 用来保存原始图像
    srcDn = imgDn.copy()
    h, w = 20, 40
    h_Up, w_Up, _ = imgUp.shape
    colIndexUp = w_Up - w  # 都是窗口的上边界的行索引
    findUp, findDn = 0, 0
    wdsnum = 1
    # 1. 搜索窗口迭代
    while findUp == 0:
        wdsUp = imgUp[0: h, colIndexUp: colIndexUp + w]  # 行区间、列区间
        cv.rectangle(imgUp, (colIndexUp, 0), (colIndexUp + w - 1, h - 1), (0, 255, 0), 1)  # 在图像内部 最外圈
        Red = wdsUp[:, :, 2]  # B0 G1 R2
        _, thrRed = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        thrRed = cv.medianBlur(thrRed, 3)
        '''
        # 查看图像
        cv.imshow("thrRed_Up", thrRed)
        cv.waitKey(0)
        cv.destroyAllWindows()
        '''
        # 判断当前窗口图像  是否存在光条
        sumY = np.sum(thrRed, axis=0)  # 0 列相加, 变成一行
        sum = np.sum(sumY, axis=0)  # 0 行相加
        if sum != 0:
            findUp = 1
            sumYIndex = np.where(sumY > 0)  # 返回（索引+ 数据类型 ）
            colUpIdx = max(sumYIndex[0])
            findColUp = colIndexUp + colUpIdx
            print("边缘点搜索：上窗口检测到光条，最右列索引：", findColUp)
            # 定位窗口
            cv.rectangle(imgUp, (findColUp - 40, 0), (findColUp + 19, 19), (255, 0, 255), 1)  # 在图像内部最外圈（宽20 高60）
            egdeWdsUp = srcUp[0:20, findColUp - 40: findColUp + 20]  # 高20  宽 60
        else:
            print("边缘点搜索：当前窗口%d未检测到光条" % wdsnum)
        colIndexUp -= w
        wdsnum += 1

    wdsnum = 1
    h_Dn, w_Dn, _ = imgDn.shape
    colIndexDn = w_Dn - w
    while findDn == 0:
        wdsDn = imgDn[h_Dn - h: h_Dn, colIndexDn: colIndexDn + w]  # 行区间、列区间
        cv.rectangle(imgDn, (colIndexDn, h_Dn - h), (colIndexDn + w - 1, h_Dn - 1), (0, 255, 0), 1)  # 在图像内部 最外圈
        Red = wdsDn[:, :, 2]  # B0 G1 R2
        _, thrRed = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        thrRed = cv.medianBlur(thrRed, 3)
        '''
        # 查看图像
        cv.imshow("thrRed_Dn", thrRed)
        cv.waitKey(0)
        cv.destroyAllWindows()
        '''
        # 判断当前窗口图像  是否存在光条
        sumY = np.sum(thrRed, axis=0)  # 0 列相加, 变成一行
        sum = np.sum(sumY, axis=0)  # 0 行相加
        if sum != 0:
            findDn = 1
            sumYIndex = np.where(sumY > 0)  # 返回（索引+ 数据类型 ）
            colDnIdx = max(sumYIndex[0])
            findColDn = colIndexDn + colDnIdx
            print("边缘点搜索：下窗口检测到光条，最右列索引：", findColDn)
            # 定位窗口
            # 在图像内部最外圈（宽20 高60）
            cv.rectangle(imgDn, (findColDn - 40, h_Dn - h), (findColDn + 19, h_Dn - 1), (255, 0, 255), 1)
            egdeWdsDn = srcDn[h_Dn - 20:h_Dn, findColDn - 40: findColDn + 20]
        else:
            print("边缘点搜索：当前窗口%d未检测到光条" % wdsnum)
        colIndexDn -= w
        wdsnum += 1

    cv.imshow("egdeWdsUp", egdeWdsUp)  # 上图的第一个窗口
    cv.imshow("egdeWdsDn", egdeWdsDn)  # 下图的第一个窗口

    # ---------------------对上图进行迭代------------------------------
    h_Up, w_Up, _ = imgUp.shape
    num = 1  # 记录迭代窗口号
    roiCur = egdeWdsUp  # 初始化 将找到的上边缘窗口初始化为首个roiCur
    roiLeftcolidx = findColUp - 40  # roi的最左边在全图中的列索引 = 上窗口光条的最右列索引 - 40
    oriUp_col = 0
    oriUp_row = 0
    while 1:
        XOR1_Red = roiCur[:, :, 2]  # B0 G1 R2
        _, XOR1 = cv.threshold(XOR1_Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        cv.imwrite(r"F:\MVS_Data\XOR1.png", XOR1)
        # 求最后的光条列坐标
        lightList = (np.sum(XOR1, axis=0))  # 0 列相加, 变成一行
        ret = np.where(lightList > 0)  # 返回（索引+ 数据类型 ）
        lightIdx = ret[0]
        # print("lightIdx:", lightIdx)
        rightShift = np.roll(lightIdx, 1)
        # print("rightShift:", rightShift)
        differList = lightIdx - rightShift
        # print("differList:", differList)
        maxValue = max(differList)
        if maxValue > 10:
            temp = differList.tolist()
            colIdx_inroi = lightIdx[temp.index(maxValue)]
        elif maxValue == 1:
            colIdx_inroi = lightIdx[0]

        # 最后的光条列坐标，也就是 在原图中的下一个窗口的起点坐标（）
        StartIdx = roiLeftcolidx + colIdx_inroi  # StartIdx：在全图中的光条的最左边光点列索引 —— roi的左边索引，加上roi的最左边在全图中的列索引
        roiNext = srcUp[num * 20:(num + 1) * 20, StartIdx - 40: StartIdx + 20]
        # 画出下一个roi的边界
        cv.rectangle(imgUp, (StartIdx - 40, num * 20), (StartIdx + 19, (num + 1) * 20 - 1), (0, 255, 0), 1)
        XOR2_Red = roiNext[:, :, 2]  # B0 G1 R2
        _, XOR2 = cv.threshold(XOR2_Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        cv.imwrite(r"F:\MVS_Data\XOR2.png", XOR2)

        dst = cv.bitwise_xor(XOR1, XOR2)  # 相同取0，相反取1，例如1 xor 1 = 0， 0 xor 0 = 1， 1 xor 0 =1
        dst = cv.medianBlur(dst, 3)  # 中值滤波
        cv.imshow("bitwise_xor", dst)
        cv.imwrite(r"F:\MVS_Data\bitwise_xor.png", dst)
        # 计算 差异图的像素点数
        sumY = np.sum(dst, axis=0)  # 0 列相加, 变成1行
        xorNum = np.sum(sumY, axis=0) / 255
        print("上：两个窗口差异像素总数：", xorNum)

        if xorNum > 100:
            print("当前窗口 发生光条间断")
            # 给间断窗口 标记红框
            cv.rectangle(imgUp, (StartIdx - 40, num * 20), (StartIdx + 19, (num + 1) * 20 - 1), (0, 0, 255), 1)
            # 标记最后的 边缘点大窗口
            # 上一个窗口的上边界索引：(num - 1) * 20   下边界索引：(num + 1) * 20 - 1
            # 上一个窗口的右边界索引：roiLeftcolidx + 59    左边界索引：roiLeftcolidx - 60
            cv.rectangle(imgUp, (roiLeftcolidx - 60, (num - 1) * 20), (roiLeftcolidx + 59, (num + 1) * 20 - 1),
                         (0, 0, 255), 1)
            finRoiUp = srcUp[(num - 1) * 20:(num + 1) * 20, roiLeftcolidx - 60: roiLeftcolidx + 60]
            oriUp_col = roiLeftcolidx - 60
            oriUp_row = (num - 1) * 20
            break
        else:
            print("迭代下一个窗口")
            roiCur = roiNext  # 窗口更新
            roiLeftcolidx = StartIdx - 40
            num += 1

    # ---------------------对下图进行迭代------------------------------
    h_Dn, w_Dn, _ = imgDn.shape
    num = 1  # 记录迭代窗口号
    roiCur = egdeWdsDn  # 初始化 将找到的上边缘窗口初始化为首个roiCur
    roiLeftcolidx = findColDn - 40  # roi的最左边在全图中的列索引 = 上窗口光条的最右列索引 - 40
    oriDn_col = 0
    oriDn_row = 0
    while 1:
        XOR1_Red = roiCur[:, :, 2]  # B0 G1 R2
        _, XOR1 = cv.threshold(XOR1_Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        cv.imwrite(r"F:\MVS_Data\XOR1_Dn.png", XOR1)
        # 求最后的光条列坐标
        lightList = (np.sum(XOR1, axis=0))  # 0 列相加, 变成一行
        ret = np.where(lightList > 0)  # 返回（索引+ 数据类型 ）
        lightIdx = ret[0]
        # print("lightIdx:", lightIdx)
        rightShift = np.roll(lightIdx, 1)
        # print("rightShift:", rightShift)
        differList = lightIdx - rightShift
        # print("differList:", differList)
        maxValue = max(differList)
        if maxValue > 10:
            temp = differList.tolist()
            colIdx_inroi = lightIdx[temp.index(maxValue)]
        elif maxValue == 1:
            colIdx_inroi = lightIdx[0]

        # 最后的光条列坐标，也就是 在原图中的下一个窗口的起点坐标（）
        StartIdx = roiLeftcolidx + colIdx_inroi  # StartIdx：在全图中的光条的最左边光点列索引 —— roi的左边索引，加上roi的最左边在全图中的列索引
        roiNext = srcDn[h_Dn - (num + 1) * 20:h_Dn - num * 20, StartIdx - 40: StartIdx + 20]
        # 画出下一个roi的边界
        cv.rectangle(imgDn, (StartIdx - 40, h_Dn - (num + 1) * 20), (StartIdx + 19, h_Dn - num * 20 - 1), (0, 255, 0),
                     1)
        XOR2_Red = roiNext[:, :, 2]  # B0 G1 R2
        _, XOR2 = cv.threshold(XOR2_Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        cv.imwrite(r"F:\MVS_Data\XOR2_Dn.png", XOR2)

        dst = cv.bitwise_xor(XOR1, XOR2)  # 相同取0，相反取1，例如1 xor 1 = 0， 0 xor 0 = 1， 1 xor 0 =1
        dst = cv.medianBlur(dst, 3)  # 中值滤波
        cv.imshow("bitwise_xor_Dn", dst)
        cv.imwrite(r"F:\MVS_Data\bitwise_xor_Dn.png", dst)
        # 计算 差异图的像素点数
        sumY = np.sum(dst, axis=0)  # 0 列相加, 变成1行
        xorNum = np.sum(sumY, axis=0) / 255
        print("下：两个窗口差异像素总数：", xorNum)

        if xorNum > 100:
            print("当前窗口 发生光条间断")
            # 给间断窗口 标记红框
            cv.rectangle(imgDn, (StartIdx - 40, h_Dn - (num + 1) * 20), (StartIdx + 19, h_Dn - num * 20 - 1),
                         (0, 0, 255), 1)
            # 标记最后的 边缘点大窗口
            # 上一个窗口的下边界索引：h_Dn - num * 20 + 19   上边界索引：h_Dn - (num + 1) * 20
            # 上一个窗口的右边界索引：roiLeftcolidx + 59    左边界索引：roiLeftcolidx - 60
            cv.rectangle(imgDn, (roiLeftcolidx - 60, h_Dn - (num + 1) * 20), (roiLeftcolidx + 59, h_Dn - num * 20 + 19),
                         (0, 0, 255), 1)
            finRoiDn = srcDn[h_Dn - (num + 1) * 20:h_Dn - num * 20 + 20, roiLeftcolidx - 60: roiLeftcolidx + 60]
            oriDn_col = roiLeftcolidx - 60
            oriDn_row = h_Dn - (num + 1) * 20
            break
        else:
            print("迭代下一个窗口")
            roiCur = roiNext  # 窗口更新
            roiLeftcolidx = StartIdx - 40
            num += 1

    # 对 最后窗口进行边缘点定位
    finRoiUp_Red = finRoiUp[:, :, 2]  # B0 G1 R2
    _, finRoiUp = cv.threshold(finRoiUp_Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
    # 求最后的光条列坐标
    lightList = (np.sum(finRoiUp, axis=0))  # 0 列相加, 变成一行
    ret = np.where(lightList > 0)  # 返回（索引+ 数据类型 ）
    lightIdx = ret[0]
    print("lightIdx:", lightIdx)
    rightShift = np.roll(lightIdx, -1)
    print("rightShift:", rightShift)
    differList = lightIdx - rightShift
    print("differList:", differList)
    minValue = min(differList)
    temp = differList.tolist()
    colUp = lightIdx[temp.index(minValue)]
    # 计算中心点行坐标
    n = 0  # 存储该列有无光点
    sum_RowIdx = 0  # 该列的中心点行索引
    for row in range(40):
        if finRoiUp[row, colUp] != 0:
            n += 1
            sum_RowIdx += row + 1
    RowUp = int(sum_RowIdx / n) - 1
    realRow_Up = oriUp_row + RowUp  # 在原始图像中的行坐标
    realCol_Up = oriUp_col + colUp
    cv.circle(imgUp, (realCol_Up, realRow_Up), 1, (255, 0, 255), -1)

    finRoiDn_Red = finRoiDn[:, :, 2]  # B0 G1 R2
    _, finRoiDn = cv.threshold(finRoiDn_Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
    # 求最后的光条列坐标
    lightList = (np.sum(finRoiDn, axis=0))  # 0 列相加, 变成一行
    ret = np.where(lightList > 0)  # 返回（索引+ 数据类型 ）
    lightIdx = ret[0]
    print("lightIdx:", lightIdx)
    rightShift = np.roll(lightIdx, -1)
    print("rightShift:", rightShift)
    differList = lightIdx - rightShift
    print("differList:", differList)
    minValue = min(differList)
    temp = differList.tolist()
    colDn = lightIdx[temp.index(minValue)]
    # 计算中心点行坐标
    n = 0  # 存储该列有无光点
    sum_RowIdx = 0  # 该列的中心点行索引
    for row in range(40):
        if finRoiDn[row, colDn] != 0:
            n += 1
            sum_RowIdx += row + 1
    RowDn = int(sum_RowIdx / n) - 1
    realRow_Dn = oriDn_row + RowDn  # 在原始图像中的行坐标
    realCol_Dn = oriDn_col + colDn
    cv.circle(imgDn, (realCol_Dn, realRow_Dn), 1, (255, 0, 255), -1)
    print("上边缘点列，行坐标：", (realCol_Up, realRow_Up))
    print("下边缘点列，行坐标：", (realCol_Dn, realRow_Dn))

    cv.imshow("imgUp", imgUp)
    cv.imshow("imgDn", imgDn)
    cv.imwrite(r"F:\MVS_Data\imgUp.png", imgUp)
    cv.imwrite(r"F:\MVS_Data\imgDn.png", imgDn)
    return (realCol_Up, realRow_Up), (realCol_Dn, realRow_Dn)


def roiIteration(binaryWeldment, CrossPoint, a):
    h, w = binaryWeldment.shape
    # 显示中心点位置
    cpShow = np.zeros([h, w, 3], np.uint8)
    cpShow[:, :, 0] = binaryWeldment
    cpShow[:, :, 1] = binaryWeldment
    cpShow[:, :, 2] = binaryWeldment

    if binaryWeldment[CrossPoint[0] - 1, CrossPoint[1] - 1] and binaryWeldment[CrossPoint[0] + 1, CrossPoint[1] + 1]:
        mode = "laserL1"
        print("正在迭代 laserL1")
    else:
        mode = "laserL2"
        print("正在迭代 laserL2")

    cp = []
    if mode == "laserL1":
        # 先找到第一个光条中心点的坐标（排除开始的某些列空着）
        for Row in range(h - 1, -1, -1):
            sum_ColIdx = 0
            n = 0
            for Col in range(w):
                if binaryWeldment[Row, Col] != 0:
                    n += 1
                    sum_ColIdx += Col + 1
            if n:
                gravityColFirst = int(sum_ColIdx / n) - 1
                gravityRowFirst = Row
                break
        Py, Px = gravityRowFirst, gravityColFirst
        n = 0
        while Py:  # 第一个点的列Px在第一列，坐标为0，故不会进去循环
            n += 1
            if n == 1:
                Droi = binaryWeldment[Py - a:Py - 1, Px - 2 * a:]
                gravityCoorList = cog_Roi(Droi, (Py, Px), a, mode)
                cp.extend(gravityCoorList)
                cv.rectangle(cpShow, (Px - 2 * a, Py - a), (w - 1, Py - 1), (0, 255, 0), 1)
            else:
                if Px - 2 * a >= 0:  # 下一个ROi
                    Droi = binaryWeldment[Py - a:Py - 1, Px - 2 * a:Px + a]
                    gravityCoorList = cog_Roi(Droi, (Py, Px), a, mode)
                    cp.extend(gravityCoorList)
                    if len(gravityCoorList) == 0:
                        break
                    else:
                        cv.rectangle(cpShow, (Px - 2 * a, Py - a), (Px + a, Py - 1), (0, 255, 0), 1)
                else:  # 下一个ROi
                    Droi = binaryWeldment[Py - a:Py - 1, :Px + a]  # 在起点的下一行
                    gravityCoorList = cog_Roi(Droi, (Py, Px), a, mode)
                    cp.extend(gravityCoorList)
                    if len(gravityCoorList) == 0:
                        break
                    else:
                        cv.rectangle(cpShow, (0, Py - a), (Px + a, Py - 1), (0, 255, 0), 1)
            Py, Px = gravityCoorList[-1][0], gravityCoorList[-1][1]  # 更新下一个起点
    if mode == "laserL2":
        # 先找到第一个光条中心点的坐标（排除开始的某些列空着）
        for Row in range(h):
            sum_ColIdx = 0
            n = 0
            for Col in range(w):
                if binaryWeldment[Row, Col] != 0:
                    n += 1
                    sum_ColIdx += Col + 1
            if n:
                gravityColFirst = int(sum_ColIdx / n) - 1
                gravityRowFirst = Row
                break
        Py, Px = gravityRowFirst, gravityColFirst
        n = 0
        while Px:
            n += 1
            if n == 1:
                Droi = binaryWeldment[Py + 1:Py + a, Px - 2 * a:]
                gravityCoorList = cog_Roi(Droi, (Py, Px), a, mode)
                cp.extend(gravityCoorList)
                cv.rectangle(cpShow, (Px - 2 * a, Py + 1), (w - 1, Py + a), (0, 255, 0), 1)
            else:
                if Px - 2 * a >= 0:  # 下一个ROi
                    Droi = binaryWeldment[Py + 1:Py + a, Px - 2 * a:Px + a]
                    gravityCoorList = cog_Roi(Droi, (Py, Px), a, mode)
                    cp.extend(gravityCoorList)
                    if len(gravityCoorList) == 0:
                        break
                    else:
                        cv.rectangle(cpShow, (Px - 2 * a, Py + 1), (Px + a, Py + a), (0, 255, 0), 1)
                else:  # 下一个ROi
                    Droi = binaryWeldment[Py + 1:Py + a, :Px + a]  # 在起点的下一行
                    gravityCoorList = cog_Roi(Droi, (Py, Px), a, mode)
                    cp.extend(gravityCoorList)
                    if len(gravityCoorList) == 0:
                        break
                    else:
                        cv.rectangle(cpShow, (0, Py + 1), (Px + a, Py + a), (0, 255, 0), 1)
            Py, Px = gravityCoorList[-1][0], gravityCoorList[-1][1]  # 更新下一个起点
    cv.imshow("roi_Iteration_" + mode, cpShow)
    cv.imwrite("F:\\MVS_Data\\roi_Iteration_" + mode + ".png", cpShow)

    cp_row, cp_col = [], []
    for i in range(len(cp)):
        cp_row.append(cp[i][0])
        cp_col.append(cp[i][1])

    # 显示中心点位置
    cpShow = np.zeros([h, w, 3], np.uint8)
    cpShow[:, :, 0] = binaryWeldment
    cpShow[:, :, 1] = binaryWeldment
    cpShow[:, :, 2] = binaryWeldment
    for i in range(0, len(cp_row)):
        cpShow[cp_row[i], cp_col[i]] = [0, 0, 255]
    cv.imshow("roiIteration_cpShow_" + mode, cpShow)
    cv.imwrite("F:\\MVS_Data\\roiIteration_cpShow_" + mode + ".png", cpShow)
    return cp_row, cp_col


def gapDetection(binary, axis, mode):
    """
    简单的通过某一行、列像素是否全部为0判断是否为间断，而不再需要计算中心点
    :param binary: 传入的二值图像（是opencv处理的，已经是numpy.nd类型）
    :return: 间断点所在的列索引
    """
    sumX = np.sum(binary, axis=axis)  # 0垂直求和  1 水平求和
    sumXIndex = np.where(sumX != 0)  # 返回索引numpy数组和元素类型的元组
    sumXIndex = sumXIndex[0]
    # print(sumXIndex)

    # 模式1 ：检测最大的一个间隙，并返回最大间隙左侧的点（工件边界点）
    if mode == 0:
        gapMax = 0
        for i in range(0, len(sumXIndex) - 1):
            gapVal = sumXIndex[i + 1] - sumXIndex[i]
            if gapVal > gapMax:
                gapMax = gapVal
                gapMaxIdxS = sumXIndex[i]  # 找到最大间隙右侧的点（工件边界点）
                gapMaxIdxL = sumXIndex[i + 1]
        print("gapMaxIdxS:", gapMaxIdxS)
        print("gapMaxIdxL:", gapMaxIdxL)
        return gapMaxIdxS, gapMaxIdxL

    # 模式2 :检测前两个最大的间隙
    if mode == 1:
        gapMax = 0
        gapSecond = 0
        gapTemp = 0
        count = 1
        for i in range(0, len(sumXIndex) - 1):
            gapVal = sumXIndex[i + 1] - sumXIndex[i]
            # 需要记录一上来就是最大间隙的情况
            if gapVal > gapSecond:
                if count == 1:
                    gapMax = gapVal
                    gapMaxIdx = sumXIndex[i]
                else:
                    if gapVal <= gapMax:
                        gapSecond = gapVal
                        gapSecondIdx = sumXIndex[i]
                    if gapVal > gapMax:
                        gapSecond = gapMax  # 最大间距
                        gapSecondIdx = gapMaxIdx
                        gapMax = gapVal
                        gapMaxIdx = sumXIndex[i]
                count += 1

        print("gapMaxIdx:", gapMaxIdx)
        print("gapSecondIdx:", gapSecondIdx)
        print("gapMax:", gapMax)
        print("gapSecond:", gapSecond)

        if gapMaxIdx > gapSecondIdx:
            leftIdx = gapSecondIdx
            rightIdx = gapMaxIdx
            gapLeft = gapSecond
            gapRight = gapMax
        else:
            leftIdx = gapMaxIdx
            rightIdx = gapSecondIdx
            gapLeft = gapMax
            gapRight = gapSecond

        return leftIdx, rightIdx, gapLeft, gapRight


def weldmentExtract(src):
    """
    workpieceExtract(src) -> retval
    @brief 分离出工件上的点集
    .   @param src：输入3通道原始条纹图像.
    .   @retval binary：中值滤波后的阈值图像.
    .   @retval cpweldmentImg：3通道的焊件中心点图像
    .   @retval cpweldment：焊件中心点集[cpweldment_row_truth, cpweldment_row, cpweldment_col]
    """
    _, _, red = cv.split(src)
    _, binary = cv.threshold(red, 195, 255, cv.THRESH_BINARY)
    binary = cv.medianBlur(binary, 3)
    # cv.imshow('binary', binary)
    cv.imwrite(r"F:\MVS_Data\binary.png", binary)

    gapMaxIdxS, gapMaxIdxL = gapDetection(binary, 0, 0)
    d1 = gapMaxIdxL - gapMaxIdxS
    binaryWeldment = binary[:, 0:int(gapMaxIdxS + d1 / 2)]
    cv.imshow('binaryWeldment', binaryWeldment)
    cv.imwrite(r"F:\MVS_Data\binaryWeldment.png", binaryWeldment)
    return binaryWeldment


def weldLine(img, featureP1, edgeP1, featureP2, edgeP2, crossP):
    print("***************** weldLine *****************")
    print("特征点坐标:", featureP1, featureP2)
    print("边界点坐标:", edgeP1, edgeP2)

    # 禁止摆放区域
    cv.circle(img, [crossP[1], crossP[0]], 40, (0, 0, 255), 2)
    cv.circle(img, [crossP[1], crossP[0]], 2, (255, 0, 255), -1)

    fp1Y, fp1X = featureP1[0], featureP1[1]  # laserL1  右下
    fp2Y, fp2X = featureP2[0], featureP2[1]  # laserL2  左下

    if fp1X < fp2X:
        print("焊缝在交叉点上"
              "方")
    else:
        print("焊缝在交叉点下方")

    """
    # singleBevel右光条由于照射方向的原因，会出现1个像素的泛光，这里是补偿误差
    if fp1X > fp2X:
        fp1Y = fp1Y - 1
    else:
        fp2Y = fp2Y - 1
    """
    bound1Y, bound1X = edgeP1[0], edgeP1[1]
    bound2Y, bound2X = edgeP2[0], edgeP2[1]

    fp_Lx, fp_Rx = min(fp1X, fp2X) - 100, max(bound1X, bound2X) + 100

    weld_k = (fp1Y - fp2Y) / (fp1X - fp2X)
    degree = math.atan(weld_k) / 3.1415926 * 180
    print("焊缝线的斜率:{:.5f}, 向下倾斜:{:.5f}°".format(weld_k, degree))

    CornerUp = 90.5
    radUp = (degree + CornerUp) * 3.1415926 / 180
    print("radUpk:", radUp)
    boundUp_k = math.tan(radUp)  # 传入的是弧度
    print("boundUp_k:", boundUp_k)

    CornerDn = 90
    boundDn_k = math.tan(degree + (180 - CornerUp))

    if weld_k != 0:
        weld_b = fp1Y - weld_k * fp1X
        fp_Ly = weld_k * fp_Lx + weld_b
        fp_Ry = weld_k * fp_Rx + weld_b
        # 绘制焊缝线
        cv.line(img, (fp_Lx, round(fp_Ly)), (fp_Rx, round(fp_Ry)), (255, 255, 0), 1)  # 青蓝
        # 绘制垂线
        boundK = -1 / weld_k

        bound_b1 = bound1Y - boundK * bound1X
        init1X = round((weld_b - bound_b1) / (boundK - weld_k))
        init1Y = round(weld_k * init1X + weld_b)
        cv.line(img, (init1X, init1Y), (bound1X, bound1Y), (0, 255, 255), 1)

        bound_b2 = bound2Y - boundUp_k * bound2X
        init2X = round((weld_b - bound_b2) / (boundUp_k - weld_k))
        init2Y = round(weld_k * init2X + weld_b)
        cv.line(img, (init2X, init2Y), (bound2X, bound2Y), (0, 255, 255), 1)
    else:
        # 绘制焊缝线
        cv.line(img, (fp_Lx, round(fp1Y)), (fp_Rx, round(fp1Y)), (255, 255, 0), 1)  # 青蓝
        # 绘制垂线
        init1X, init2X = bound1X, bound2X
        init1Y, init2Y = fp1Y, fp1Y
        cv.line(img, (init1X, init1Y), (bound1X, bound1Y), (0, 255, 255), 1)
        cv.line(img, (init2X, init2Y), (bound2X, bound2Y), (0, 255, 255), 1)

    # 标记所有特征点
    # img[fp1Y, fp1X] = [255, 0, 255]
    cv.circle(img, [fp1X, fp1Y], 1, (255, 0, 255), -1)
    cv.circle(img, [fp2X, fp2Y], 1, (255, 0, 255), -1)
    # img[bound1Y, bound1X] = [255, 0, 255]
    cv.circle(img, [bound1X, bound1Y], 1, (255, 0, 255), -1)
    # img[bound2Y, bound2X] = [255, 0, 255]
    cv.circle(img, [bound2X, bound2Y], 1, (255, 0, 255), -1)

    cv.circle(img, [init1X, init1Y], 1, (255, 0, 255), -1)
    cv.circle(img, [init1X, init1Y], 5, (255, 0, 255), 1)

    cv.circle(img, [init2X, init2Y], 1, (255, 0, 255), -1)
    cv.circle(img, [init2X, init2Y], 5, (255, 0, 255), 1)
    print("初始点1坐标:", (init1Y, init1X))
    print("初始点2坐标:", (init2Y, init2X))

    # 错位距离
    # cv.line(src, (initialP1_x, initialP1_y), (initialP2_x, initialP2_y), (0, 0, 255), 2)
    missDistance = math.sqrt((init1Y - init2Y) ** 2 + (init1X - init2X) ** 2)
    if missDistance > 20:
        print("错位距离：%0.2f , 工件未对齐" % missDistance)
    else:
        print("错位距离：%0.2f , 工件对齐" % missDistance)

    cv.imshow("Results", img)
    cv.imwrite(r"F:\MVS_Data\Results.png", img)
